#include "sabertooth.h"


void send(uint8_t command, uint8_t data)
{
  uart_putc(ADDRESS);
  uart_putc(command);
  uart_putc(data);
  uart_putc((ADDRESS + command + data) & 0b01111111);
}

void all_stop()
{
  uint8_t reverse_m1 = 1; // motor 1 forward
  uint8_t reverse_m2 = 5; // motor 2 forward

  send(reverse_m1, STOP);
  send(reverse_m2, STOP);
}

void forward(uint8_t speed)
{
  uint8_t forward_m1 = 0;
  uint8_t forward_m2 = 4;

  send(forward_m1, speed);
  send(forward_m2, speed);
}

void reverse(uint8_t speed)
{
  uint8_t reverse_m1 = 1; // command for motor 1 to go forward
  uint8_t reverse_m2 = 5; // command for motor 2 to go forward
        
  send(reverse_m1, speed);
  send(reverse_m2, speed);
}
void turnRight()
{
  uint8_t forward_m1 = 0; // command for motor 1 to go forward
  uint8_t reverse_m2 = 5; // command for motor 2 to go forward
        
  send(forward_m1, FULL);
  send(reverse_m2, HALF);
}
void turnLeft()
{
  uint8_t reverse_m1 = 1; // command for mkotor 1 to go forward
  uint8_t forward_m2 = 4; // command for motor 2 to go forward
        
  send(reverse_m1, HALF);
  send(forward_m2, FULL);
}
